//========================================================================
//  This software is free: you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License Version 3,
//  as published by the Free Software Foundation.
//
//  This software is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public License
//  Version 3 in the file COPYING that came with this distribution.
//  If not, see <http://www.gnu.org/licenses/>.
//========================================================================
/*!
\file    gui.cpp
\brief   GUI for Skeleton Pointing
\author  Joydeep Biswas, (C) 2011
*/
//========================================================================

#include "gui.h"


GLViewer::GLViewer(QWidget* parent) : QGLWidget( QGL::SampleBuffers | QGL::DoubleBuffer ,parent)
{
  connect(this, SIGNAL(postRedraw(void)), this, SLOT(redraw(void)));
  showCalibration = false;
  viewScale = 1.0;
  viewXOffset = viewYOffset = 0.0;
  
  viewAngleY = viewAngleZ = -RAD(45.0);
  kinectHeight = 0.0;
  groundPlaneNormal.zero();
  for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ ){
    skeletonFrame.SkeletonData[i].eTrackingState = NUI_SKELETON_NOT_TRACKED;
  }
  setAutoFillBackground(false);
  setMouseTracking(true);
  for(int i=0; i<4; i++)
    calibrationPoints[i].zero();
}

void GLViewer::chkShowCalibrationChanged ( int state )
{
  if(state==Qt::Checked)
    showCalibration = true;
  else
    showCalibration = false;
  postRedraw();
}

SkeletonPointingGUI::SkeletonPointingGUI(QWidget* parent) : QWidget(parent)
{
  calibrating = false;
  startCalibrationCallback = NULL;
  createGUI();
  glViewer->chkShowCalibrationChanged(Qt::Checked);
  setMouseTracking(true);
}

void SkeletonPointingGUI::createGUI()
{
  mainLayout = new QHBoxLayout;
  QSplitter * splitter = new QSplitter(Qt::Horizontal, this);
  controlPanel = new QGroupBox(tr("Control Panel"));
  QGridLayout *grid = new QGridLayout;
  calibrationBox = new QGroupBox(tr("Display Calibration"));
  controlPanel->setMaximumWidth(300);
  calibrationBox->setMaximumHeight(300);
  QVBoxLayout* calibrationBoxLayout = new QVBoxLayout;
  btnLoad = new QPushButton(tr("Load Calibration"));
  btnSave = new QPushButton(tr("Save Calibration"));
  btnCalibrate = new QPushButton(tr("Start Calibration"));
  optPoint[0] = new QRadioButton(tr("Calibration Point 1"));
  optPoint[1] = new QRadioButton(tr("Calibration Point 2"));
  optPoint[2] = new QRadioButton(tr("Calibration Point 3"));
  optPoint[0]->setChecked(true);
  chkShowCalibration = new QCheckBox(tr("Show Calibration Result"));
  calibrationBoxLayout->addWidget(optPoint[0]);
  calibrationBoxLayout->addWidget(optPoint[1]);
  calibrationBoxLayout->addWidget(optPoint[2]);
  calibrationBoxLayout->addWidget(btnCalibrate);
  calibrationBoxLayout->addWidget(chkShowCalibration);
  calibrationBoxLayout->addWidget(btnLoad);
  calibrationBoxLayout->addWidget(btnSave);
  calibrationBox->setLayout(calibrationBoxLayout);
  grid->addWidget(calibrationBox);
  controlPanel->setLayout(grid);

  glViewer = new GLViewer();
  
  splitter->addWidget(controlPanel);
  splitter->addWidget(glViewer);
  mainLayout->addWidget(splitter);
  setLayout(mainLayout);

  //Connect signals to slots
  connect(chkShowCalibration, SIGNAL(stateChanged(int)), glViewer, SLOT(chkShowCalibrationChanged(int)));
  connect(btnCalibrate, SIGNAL(clicked(bool)), this, SLOT(btnCalibrateClicked(bool)));
  connect(btnLoad, SIGNAL(clicked(bool)), this, SLOT(btnCalibrationLoadSave(bool)));
  connect(btnSave, SIGNAL(clicked(bool)), this, SLOT(btnCalibrationLoadSave(bool)));
  chkShowCalibration->setCheckState(Qt::Checked);
}

void SkeletonPointingGUI::setCallbacks(void (*_startCalibrationCallback)(int), void (*_stopCalibrationCallback)(), void (*_loadSaveCallback)(bool))
{
  startCalibrationCallback = _startCalibrationCallback;
  stopCalibrationCallback = _stopCalibrationCallback;
  calibrationLoadSave = _loadSaveCallback;
}

bool SkeletonPointingGUI::isCalibrating()
{
  return calibrating;
}

void SkeletonPointingGUI::btnCalibrationLoadSave(bool checked)
{
  if(!calibrationLoadSave)
    return;
  if(((QPushButton*)sender())->text().compare(tr("Load Calibration"))==0)
    calibrationLoadSave(true);
  else
    calibrationLoadSave(false);
}

void SkeletonPointingGUI::btnCalibrateClicked(bool checked)
{
  guiMutex.lock();
  calibrating = !calibrating;
  //Determine which point to calibrate
  int selectedPoint = -1;
  for(int i=0; i <3 && selectedPoint <0; i++){
    if(optPoint[i]->isChecked())
      selectedPoint = i;
  }
  if(selectedPoint<0){
    //How did all of the options get unselected? Bizarre!
    //optPoint[0]->setChecked(true);
    selectedPoint = 0;
  }
  if(calibrating){
    for(int i=0; i <3; i++)
      optPoint[i]->setEnabled(false);
    btnCalibrate->setText("Stop Calibration");
  }else{
    for(int i=0; i <3; i++)
      optPoint[i]->setEnabled(true);
    //optPoint[0]->setChecked(true);
    btnCalibrate->setText("Start Calibration");
  }
  guiMutex.unlock();
  if(calibrating){
    if(startCalibrationCallback)
      startCalibrationCallback(selectedPoint);
  }else{
    if(stopCalibrationCallback)
      stopCalibrationCallback();
  }
}

void GLViewer::mousePressEvent(QMouseEvent* event)
{
  leftButton = event->buttons().testFlag(Qt::LeftButton);
  midButton = event->buttons().testFlag(Qt::MidButton);
  rightButton = event->buttons().testFlag(Qt::RightButton);
  bool shiftKey = event->modifiers().testFlag(Qt::ShiftModifier);
  bool ctrlKey = event->modifiers().testFlag(Qt::ControlModifier);
  
  drawingMutex.lock();
  mouseStartX = event->x();
  mouseStartY = event->y();
  this->postRedraw();
  
  if( (ctrlKey || shiftKey) && leftButton ){
    double scale = 1.0/viewScale;
    double x = (double(event->x()) - 0.5*double(width()) - viewXOffset)*scale;
    double y = -(double(event->y()) - 0.5*double(height()) - viewYOffset)*scale;
    mouseStartX = event->x();
    mouseStartY = event->y();
  }
  drawingMutex.unlock();
}

void GLViewer::mouseReleaseEvent(QMouseEvent* event)
{
  bool shiftKey = event->modifiers().testFlag(Qt::ShiftModifier);
  bool ctrlKey = event->modifiers().testFlag(Qt::ControlModifier);
  if( shiftKey && leftButton ){
    double setOrientation = atan2(double(-(event->y()-mouseStartY)),double(event->x()-mouseStartX));
    /*
    if(ptrCallback)
      ptrCallback(setLocation,setOrientation, 3);
    */
  }else if( ctrlKey && leftButton ){
    double setOrientation = atan2(double(-(event->y()-mouseStartY)),double(event->x()-mouseStartX));
    /*
    if(ptrCallback)
      ptrCallback(setLocation,setOrientation, 2);
    */
  }
}

void GLViewer::mouseMoveEvent(QMouseEvent* event)
{
  static const bool debug = false;
  bool leftButton = event->buttons().testFlag(Qt::LeftButton);
  bool midButton = event->buttons().testFlag(Qt::MidButton);
  bool rightButton = event->buttons().testFlag(Qt::RightButton);
  
  if(debug) printf("MouseMove Event, Left:%d Mid:%d Right:%d\n", leftButton?1:0, midButton?1:0, rightButton?1:0);
  
  drawingMutex.lock();
  
  if(leftButton){
    //Rotate
    viewAngleZ = angle_mod(viewAngleZ - double(event->x() - mouseStartX)*RAD(1.0));
    mouseStartX = event->x();
    viewAngleY = abs_bound(viewAngleY - double(event->y() - mouseStartY)*RAD(1.0), RAD(45.0));
    mouseStartY = event->y();
    this->postRedraw();
  }else if(rightButton){
    //Pan
    viewXOffset += double(event->x() - mouseStartX);
    viewYOffset += double(event->y() - mouseStartY);
    mouseStartX = event->x();
    mouseStartY = event->y();
    this->postRedraw();
  }else if(midButton){
    //Zoom
    double zoomRatio = -double(event->y() - mouseStartY)/500.0;
    double oldScale = viewScale;
    viewScale = viewScale*(1.0+zoomRatio);
    viewXOffset *= viewScale/oldScale;
    viewYOffset *= viewScale/oldScale;
    mouseStartX = event->x();
    mouseStartY = event->y();
    resizeGL(width(), height());
    this->postRedraw();
  }
  drawingMutex.unlock();
}

void GLViewer::wheelEvent(QWheelEvent* event)
{
  static const bool debug = false;
  double zoomRatio = double(event->delta())/1000.0;
  double oldScale = viewScale;
  drawingMutex.lock();
  viewScale = viewScale*(1.0+zoomRatio);
  viewXOffset *= viewScale/oldScale;
  viewYOffset *= viewScale/oldScale;
  if(debug) printf("Zoom: %5.3f\n",viewScale);
  resizeGL(width(), height());
  this->postRedraw();
  drawingMutex.unlock();
}

void SkeletonPointingGUI::keyPressEvent(QKeyEvent* event)
{
  static const bool debug = false;
  if(debug) printf("KeyPress: 0x%08X\n",event->key());
  if(event->key() == Qt::Key_Space)
    glViewer->resetView();
  if(event->key() == Qt::Key_Escape)
    close();
}

void GLViewer::initializeGL()
{
  glShadeModel(GL_SMOOTH);
  glEnable(GL_DEPTH_TEST);
  glDisable(GL_LIGHTING);
  glEnable (GL_BLEND);
  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
}

void GLViewer::resizeGL(int width, int height)
{
  static const double nearClipping = 0.001;
  static const double farClipping = 30.0;
  static const double minFarClipping = 15.0;
  glViewport(0, 0, width, height);
  
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  
  double fovH = RAD(49.09);
  double fovV = RAD(43.0);
  double tanHFovH = tan(fovH*0.5)*width/800.0; 
  double tanHFovV = tan(fovV*0.5)*height/600.0;
  glFrustum(-nearClipping*tanHFovH,nearClipping*tanHFovH,-nearClipping*tanHFovV,nearClipping*tanHFovV,
    nearClipping,max(minFarClipping,farClipping/viewScale));
  
  glMatrixMode(GL_MODELVIEW);
}

void GLViewer::resetView()
{
  drawingMutex.lock();
  viewScale = 1.0;
  viewXOffset = viewYOffset = 0.0;
  this->postRedraw();
  drawingMutex.unlock();
}

void GLViewer::drawGroundPlane()
{
  static const double gridSize = 6.0;
  static const double gridCellSize = 0.5;
  glBegin(GL_LINES);
  glColor3d(1.0,1.0,1.0);
  for(double x =-gridSize; x<=gridSize; x+=gridCellSize){
    glVertex3d(x, -gridSize, -1.0);
    glVertex3d(x, gridSize, -1.0);
  }
  for(double y =-gridSize; y<=gridSize; y+=gridCellSize){
    glVertex3d(-gridSize, y, -1.0);
    glVertex3d(gridSize, y, -1.0);
  }
  glEnd();
}

void GLViewer::drawKinectViewFrustrum()
{
  static const double tanHFovH = tan(0.5*RAD(57.0));
  static const double tanHFovV = tan(0.5*RAD(43.0));
  static const double maxDepth = 6.0;
  
  /*
  glLineWidth(4.0);
  glColor3f(162.0/255.0,27.0/255.0,224.0/255.0);
  drawGlLine(vector3f(0,0,0),groundPlaneNormal);
  glLineWidth(1.0);
  */

  glBegin(GL_TRIANGLES);
  glColor4d(0.105882353,0.168627451,0.878431373, 0.2);
  
  vector3d p0(1.0, -tanHFovH, -tanHFovV);
  vector3d p1(1.0, tanHFovH, -tanHFovV);
  vector3d p2(1.0, tanHFovH, tanHFovV);
  vector3d p3(1.0, -tanHFovH, tanHFovV);
  
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(V3COMP(maxDepth*p0));
  glVertex3d(V3COMP(maxDepth*p1));
  
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(V3COMP(maxDepth*p1));
  glVertex3d(V3COMP(maxDepth*p2));
  
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(V3COMP(maxDepth*p2));
  glVertex3d(V3COMP(maxDepth*p3));
  
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(V3COMP(maxDepth*p3));
  glVertex3d(V3COMP(maxDepth*p0));
  
  glEnd();

  glColor4d(0.105882353,0.168627451,0.878431373, 1.0);
  glLineWidth(2.0);
  glBegin(GL_LINES);
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(V3COMP(maxDepth*p0));
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(V3COMP(maxDepth*p1));  
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(V3COMP(maxDepth*p2));  
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(V3COMP(maxDepth*p3));
  glVertex3d(V3COMP(maxDepth*p0));
  glVertex3d(V3COMP(maxDepth*p1));
  glVertex3d(V3COMP(maxDepth*p1));
  glVertex3d(V3COMP(maxDepth*p2));
  glVertex3d(V3COMP(maxDepth*p2));
  glVertex3d(V3COMP(maxDepth*p3));
  glVertex3d(V3COMP(maxDepth*p0));
  glVertex3d(V3COMP(maxDepth*p3));
  glEnd();
  glLineWidth(1.0);
}

void GLViewer::drawDisplayPlane()
{
  static const double gridSize = 6.0;
  static const double gridCellSize = 0.5;
  static const double sqMinDistanceFromOrigin = sq(0.05);
  for(int i=0; i<4; i++){
    // No point drawing the display if it has not been calibrated yet!
    if(calibrationPoints[i].sqlength()<sqMinDistanceFromOrigin)
      return;
  }
  
  glBegin(GL_QUADS);
  glColor4d(0.0509803922,0.870588235,0.149019608, 0.5); //Transparent green
  
  for(int i=0; i<4; i++)
    glVertex3d(V3COMP(calibrationPoints[i]));
  
  glEnd();

  glPointSize(10.0);
  glColor3f(0.0,1.0,1.0);
  glBegin(GL_POINTS);
  glVertex3d(V3COMP(target3d));
  glEnd();
}

template <class vec> void GLViewer::drawGlLine(vec p1, vec p2)
{
  glBegin(GL_LINES);
  glVertex3f(V3COMP(p1));
  glVertex3f(V3COMP(p2));
  glEnd();
}

void GLViewer::updateTarget(vector3d target3d)
{
  drawingMutex.lock();
  this->target3d = target3d;
  drawingMutex.unlock();
}

void GLViewer::drawSkeletons(NUI_SKELETON_FRAME skeletons)
{
  glLineWidth(3.0);
  glPointSize(6.0);
  for( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ ){
    // Query a single skeleton, if it is being tracked
    const NUI_SKELETON_DATA &skeleton = skeletons.SkeletonData[i];
    if(skeleton.eTrackingState != NUI_SKELETON_TRACKED || skeleton.dwQualityFlags!=0)
      continue;
    if( skeleton.eTrackingState == NUI_SKELETON_TRACKED ){
      vector3f hip_c   (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HIP_CENTER]));
      vector3f hip_r   (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HIP_RIGHT]));
      vector3f hip_l   (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HIP_LEFT]));
      vector3f spine        (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SPINE]));
      vector3f shoulder_c   (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_CENTER]));
      vector3f shoulder_r   (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT]));
      vector3f shoulder_l   (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT]));
      vector3f elbow_r (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_ELBOW_RIGHT]));
      vector3f elbow_l (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_ELBOW_LEFT]));
      vector3f wrist_r (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_WRIST_RIGHT]));
      vector3f wrist_l (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_WRIST_LEFT]));
      vector3f knee_r (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_KNEE_RIGHT]));
      vector3f knee_l (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_KNEE_LEFT]));
      vector3f ankle_r (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_ANKLE_RIGHT]));
      vector3f ankle_l (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_ANKLE_LEFT]));
      vector3f hand_r (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT]));
      vector3f hand_l (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HAND_LEFT]));
      vector3f foot_r (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_RIGHT]));
      vector3f foot_l (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_FOOT_LEFT]));
      vector3f head (GET_KINECT_COORDS(skeleton.SkeletonPositions[NUI_SKELETON_POSITION_HEAD]));
      
      glColor3f(1.0,1.0,1.0);
      drawGlLine(ankle_r, foot_r);
      drawGlLine(ankle_r, knee_r);
      drawGlLine(hip_r, knee_r);
      drawGlLine(hip_r, hip_c);
      
      drawGlLine(hand_r, wrist_r);
      drawGlLine(wrist_r, elbow_r);
      drawGlLine(shoulder_r, elbow_r);
      drawGlLine(shoulder_r, shoulder_c);
      
      drawGlLine(ankle_l, foot_l);
      drawGlLine(ankle_l, knee_l);
      drawGlLine(hip_l, knee_l);
      drawGlLine(hip_l, hip_c);
      
      drawGlLine(hand_l, wrist_l);
      drawGlLine(wrist_l, elbow_l);
      drawGlLine(shoulder_l, elbow_l);
      drawGlLine(shoulder_l, shoulder_c);
      
      drawGlLine(hip_c, spine);
      drawGlLine(shoulder_c, spine);
      drawGlLine(shoulder_c, head);
      
      glColor3f(1.0,0.0,0.0);
      glBegin(GL_POINTS);
      glVertex3f(V3COMP(ankle_r));
      glVertex3f(V3COMP(knee_r));
      glVertex3f(V3COMP(hip_r));
      glVertex3f(V3COMP(wrist_r));
      glVertex3f(V3COMP(elbow_r));
      glVertex3f(V3COMP(shoulder_r));
      glVertex3f(V3COMP(ankle_l));
      glVertex3f(V3COMP(knee_l));
      glVertex3f(V3COMP(hip_l));
      glVertex3f(V3COMP(wrist_l));
      glVertex3f(V3COMP(elbow_l));
      glVertex3f(V3COMP(shoulder_l));
      glVertex3f(V3COMP(hip_c));
      glVertex3f(V3COMP(spine));
      glVertex3f(V3COMP(shoulder_c));
      glVertex3f(V3COMP(head));
      glEnd();
    }        
  }

  glPointSize(2.0);
  glLineWidth(1.0);
}

void GLViewer::drawCalibrationData()
{
  static const double VectorLength = 1.0;
  glPointSize(6.0);
  glLineWidth(3.0);
  
  glColor3f(0.85098, 0.10588, 0.8784); //Purple
  glBegin(GL_POINTS);
  for(int i=0; i<dataPoints.size(); i++){
    glVertex3f(V3COMP(dataPoints[i]));
  }
  glColor3f(1.0, 0.0, 0.0); 
  glVertex3f(V3COMP(calibrationPoints[0]));
  glColor3f(0.0, 1.0, 0.0); 
  glVertex3f(V3COMP(calibrationPoints[1]));
  glColor3f(0.0, 0.0, 1.0); 
  glVertex3f(V3COMP(calibrationPoints[2]));
  glColor3f(1.0, 0.0, 1.0); 
  glVertex3f(V3COMP(calibrationPoints[3]));
  glEnd();

  glColor3f(1.0, 0.5843, 0.1686); //Orange
  for(int i=0; i<dataVectors.size(); i++){
    vector3d p0 = dataPoints[i];
    vector3d p1 = p0 + VectorLength*dataVectors[i];
    drawGlLine(p0,p1);
  }


  glPointSize(2.0);
  glLineWidth(1.0);
}

void GLViewer::drawCalibrationSetup()
{
  static const bool KinectAngleCorrection = false;
  vector3d cameraLoc = vector3d(16.0,0.0,0.0)/viewScale;
  double viewAngleX = groundPlaneNormal.y;
  
  glLoadIdentity();
  cameraLoc = cameraLoc.rotate_y(viewAngleY);
  cameraLoc = cameraLoc.rotate_z(viewAngleZ);
  gluLookAt(V3COMP(cameraLoc),
            0, 0, 0, /* look at the origin */
            0, 0, 1); /* positive Y up vector */
  
  drawGroundPlane();
  if(KinectAngleCorrection){
    glPushMatrix();
    glRotated(-viewAngleX, 0.0,1.0,0.0);
  }
  drawSkeletons(skeletonFrame);
  drawCalibrationData();
  if(KinectAngleCorrection) glPopMatrix();

  glDisable(GL_CULL_FACE);
  glDisable(GL_DEPTH_TEST);

  drawDisplayPlane();
  if(KinectAngleCorrection) glRotated(-viewAngleX, 0.0,1.0,0.0);
  drawKinectViewFrustrum();
  
  glShadeModel(GL_FLAT);
  glDisable(GL_LIGHTING);
  
}

void GLViewer::drawInstructions(QPainter* painter)
{
  QString text = tr("Drag with left mouse button to rotate, right button to pan, middle button to zoom");
  QFontMetrics metrics = QFontMetrics(font());
  int border = qMax(4, metrics.leading());
  
  QRect rect = metrics.boundingRect(0, 0, width() - 2*border, int(height()*0.125),Qt::AlignCenter | Qt::TextWordWrap, text);
  painter->setRenderHint(QPainter::TextAntialiasing);
  painter->fillRect(QRect(0, 0, width(), rect.height() + 2*border),QColor(0, 0, 0, 127));
  painter->setPen(Qt::white);
  painter->fillRect(QRect(0, 0, width(), rect.height() + 2*border),QColor(0, 0, 0, 127));
  painter->drawText((width() - rect.width())/2, border,
                   rect.width(), rect.height(),
                   Qt::AlignCenter | Qt::TextWordWrap, text);
}

void GLViewer::paintEvent(QPaintEvent* event)
{
  //FunctionTimer ft("GLViewer::paintEvent");
  
  if(!drawingMutex.tryLock()){
    return;
  }
  makeCurrent();
  glShadeModel(GL_SMOOTH);
  glEnable(GL_DEPTH_TEST);
  glDisable(GL_LIGHTING);
  glEnable (GL_BLEND);
  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  

  glClearColor(0.0,0.0,0.0,1.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  
  if(showCalibration)
    drawCalibrationSetup();
  
  glMatrixMode(GL_MODELVIEW);
  glPopMatrix();
  
  QPainter painter(this);
  drawInstructions(&painter);
  painter.end();

  drawingMutex.unlock();
  
}

void GLViewer::updateData(NUI_SKELETON_FRAME _skeletonFrame)
{
  drawingMutex.lock();
  skeletonFrame = _skeletonFrame;
  groundPlaneNormal = vector3f(GET_KINECT_COORDS(skeletonFrame.vFloorClipPlane));
  kinectHeight = skeletonFrame.vFloorClipPlane.w;
  drawingMutex.unlock();
  postRedraw();
}

void GLViewer::resizeEvent(QResizeEvent* event)
{
  QGLWidget::resizeEvent(event);
  drawingMutex.lock();
  this->postRedraw();
  drawingMutex.unlock();
}

void SkeletonPointingGUI::calibrationStatusUpdate(vector3d calibrationPoints[4],
  vector<vector3d>& _dataPoints, vector<vector3d>& _dataVectors)
{
  glViewer->calibrationStatusUpdate(calibrationPoints,_dataPoints, _dataVectors);
}

void GLViewer::calibrationStatusUpdate(vector3d _calibrationPoints[4],
  vector<vector3d>& _dataPoints, vector<vector3d>& _dataVectors)
{
  drawingMutex.lock();
  for(int i=0; i<4; i++)
    calibrationPoints[i] = _calibrationPoints[i];
  dataPoints = _dataPoints;
  dataVectors = _dataVectors;
  drawingMutex.unlock();
}